#include "ros/ros.h"
#include "nav_msgs/Odometry.h"
#include "tf2/LinearMath/Quaternion.h"
#include<geometry_msgs/Quaternion.h>
#include<geometry_msgs/PoseStamped.h>
#include<geometry_msgs/Twist.h>
#include<tf/tf.h>
#include <Eigen/Eigen>
#include <Eigen/Core>
#include <Eigen/StdVector>
#include "iostream"
#include "algorithm"
#include "cmath"
using namespace std;

ros::Publisher pub;

typedef struct  p_robot
{
    Eigen::Vector3d Pose;
    Eigen::Matrix3d Rotation;

    double theat;
}P_robot;

P_robot p_start;
P_robot p_now;

    //给定目标点
    double goal_x;
    double goal_y;  
    double goal_theat;
    double dtheat;
    bool isstart;
    bool flag_theat=true;
    bool get_point=true;
    bool startornot;
    double  goal_theat_start;
    double goal_odom_x;
    double goal_odom_y;
    double d_x;
    double d_y;
    bool flag_goal=true;

    

void odomCallback(const nav_msgs::OdometryConstPtr& odom)
{   
    p_now.Rotation = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                                          odom->pose.pose.orientation.y, odom->pose.pose.orientation.z).toRotationMatrix();
    double roll, pitch, yaw;
    tf::Quaternion q;
    tf::quaternionMsgToTF(odom->pose.pose.orientation,q);
    tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
    p_now.theat=yaw;
    cout<<"yaw  is: "<<p_now.theat<<endl;
    p_now.Pose(0) = odom->pose.pose.position.x;
    p_now.Pose(1) = odom->pose.pose.position.y;
    p_now.Pose(2) = odom->pose.pose.position.z;
    cout<<"x,y is: "<<p_now.Pose(0)<<" ,"<<p_now.Pose(1)<<endl;

      if(get_point==true)
    {
        p_start.Rotation = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x,
                                     odom->pose.pose.orientation.y, odom->pose.pose.orientation.z).toRotationMatrix();
        double roll, pitch, yaw;
        tf::Quaternion q;
        tf::quaternionMsgToTF(odom->pose.pose.orientation,q);
        tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
        p_start.theat=yaw;
        p_start.Pose(0) = odom->pose.pose.position.x;
        p_start.Pose(1) = odom->pose.pose.position.y;
        p_start.Pose(2) = odom->pose.pose.position.z;
        
        goal_theat=atan2(goal_y-p_start.Pose(1)+0.175*sin(p_start.theat),goal_x-p_start.Pose(0)+0.175*cos(p_start.theat));
        if(startornot==true)
        {
            goal_theat=atan2(goal_y-p_start.Pose(1),goal_x-p_start.Pose(0)+0.175);
        }
        goal_theat_start=goal_theat;
        get_point=false;
    }
    
    double v=0.0;
    double w=0.0;
    dtheat=fabs(goal_theat-p_now.theat);
    geometry_msgs::Twist twist;
    if(dtheat>0.05&&flag_theat==true)
    {
        w=0.5*(fabs(goal_theat)/goal_theat);
        twist.linear.x=v;
        twist.angular.z=w;
       pub.publish(twist);
        return;
    }
    flag_theat=false;

    goal_theat=atan2(goal_y-p_now.Pose(1)+0.175*sin(p_now.theat),goal_x-p_now.Pose(0)+0.175*cos(p_now.theat));
    twist.linear.x=0.2;
    twist.angular.z=100*(goal_theat-p_now.theat);
    if(abs(twist.angular.z)>0.2)
    {
        twist.angular.z=0.15*abs(twist.angular.z)/twist.angular.z;
    }

    goal_odom_x = goal_x + 0.175*cos(goal_theat_start);
    goal_odom_y = goal_y + 0.175*sin(goal_theat_start);

    d_x=abs(goal_odom_x-p_now.Pose(0));
    d_y=abs(goal_odom_y-p_now.Pose(1));
  
    if(d_x<0.06&&d_y<0.06)
    {
        twist.angular.z=0.0;
        twist.linear.x=0.0;
        cout<<"-----到达目标点------"<<endl;
        flag_goal=false;
    }
    if (flag_goal)
    {
         pub.publish(twist);
    }

 }

int main(int argc, char *argv[])
{
    cout<<"请输入X方向的位置：";
    cin>>goal_x;
    cout<<"请输入Y方向的位置：";
    cin>>goal_y;
    cout<<"是否为起始点:";
    cin>>isstart;
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"test_point");
    ros::NodeHandle nh;

    //订阅odom消息
    //发布速度控制消息
     pub=nh.advertise<geometry_msgs::Twist>("/mobile_base/commands/velocity",100);
     ros::Subscriber tt = nh.subscribe("/t265/odom/sample",100, odomCallback);

     ros::spin();
    return 0;
}
